﻿using System;
using System.Collections.Generic;
using Common;
using ImageProcessing.Common;

namespace ImageProcessing.Detectors
{

    // note : this was derived from openCV and openSurf and recompiled for wp7, so credit must go to those developers

    public class FastHessian : IInterestPointDetector
    {
        private readonly float _threshold;
        private readonly int _octaves;
        private readonly int _sampleSize;
        private readonly IntegralImage _integralImage;
        private readonly int[,] _filterMap = { { 0, 1, 2, 3 }, { 1, 3, 4, 5 }, { 3, 5, 6, 7 }, { 5, 7, 8, 9 }, { 7, 9, 10, 11 } };
        private readonly List<InterestPoint> _ipts = new List<InterestPoint>();
        private readonly int _width;
        private readonly int _height;

        public IntegralImage IntegralImage
        {
            get { return _integralImage; }
        }

        public FastHessian(float threshold, int octaves, int sampleSize, int width, int height, IntegralImage integralImage)
        {
            _threshold = threshold;
            _octaves = octaves;
            _sampleSize = sampleSize;
            _width = width;
            _height = height;
            _integralImage = integralImage;
        }
           
        public List<InterestPoint> GetInterestPoints(ref byte[] yFrame)
        {
            // create integral image
            _integralImage.BuildIntegralImage(ref yFrame);

            return GetInterestPoints();
        }
        
        public List<InterestPoint> GetInterestPoints(ref int[] argbFrame)
        {
            // create integral image
            _integralImage.BuildIntegralImage(ref argbFrame);

            return GetInterestPoints();
        }

        private List<InterestPoint> GetInterestPoints()
        {
            _ipts.Clear();

            // create response map
            var responseMaps = BuildResponseMaps(_width, _height);

            // get response layers
            for (var o = 0; o < _octaves; o++)
                for (var i = 0; i <= 1; i++)
                {
                    var b = responseMaps[_filterMap[o, i]];
                    var m = responseMaps[_filterMap[o, i + 1]];
                    var t = responseMaps[_filterMap[o, i + 2]];

                    // loop over the middle reponse layer at densithy of the most  sparse layer(always top), to find maxima across scale and space
                    var tHeight = t.Height;
                    var tWidth = t.Width;
                    for (int r = 0; r < tHeight; ++r)
                        for (int c = 0; c < tWidth; ++c)
                            if (IsExtremum(r, c, t, m, b))
                                InterpolateExtermum(r, c, t, m, b);

                }

            return _ipts;
        }

        /// <summary>
        /// Interplate scale-space extrema to subpixel accurace fo from an image feature
        /// </summary>
        /// <param name="row"></param>
        /// <param name="col"></param>
        /// <param name="topRL"></param>
        /// <param name="middleRL"></param>
        /// <param name="bottomRL"></param>
        private void InterpolateExtermum(int row, int col, ResponseLayer topRL, ResponseLayer middleRL, ResponseLayer bottomRL)
        {
            Matrix d = Matrix.Create(BuildDerivative(row, col, topRL, middleRL, bottomRL));
            Matrix h = Matrix.Create(BuildHessian(row, col, topRL, middleRL, bottomRL));
            Matrix hi = h.Inverse();
            Matrix of = -1 * hi * d;

            // get the offset from the interpolation
            double[] o = { of[0, 0], of[1, 0], of[2, 0] };

            // get the step distance between the filters
            int filterStep = (middleRL.Filter - bottomRL.Filter);

            // If point is sufficiently close to the actual extremum
            if (Math.Abs(o[0]) < 0.5f && Math.Abs(o[1]) < 0.5f && Math.Abs(o[2]) < 0.5f)
            {
                var ipt = new InterestPoint(new PointInfo());
                ipt.x = (float)((col + o[0]) * topRL.Step);
                ipt.y = (float)((row + o[1]) * topRL.Step);
                ipt.Scale = (float)((0.1333f) * (middleRL.Filter + o[2] * filterStep));
                ipt.Sign = (int)(middleRL.GetLaplacian(row, col, topRL));
                _ipts.Add(ipt);
            }

        }

        private double[,] BuildHessian(int row, int col, ResponseLayer topRL, ResponseLayer middleRL, ResponseLayer bottomRL)
        {
            double v = middleRL.GetResponse(row, col, topRL);
            double dxx = middleRL.GetResponse(row, col + 1, topRL) + middleRL.GetResponse(row, col - 1, topRL) - 2 * v;
            double dyy = middleRL.GetResponse(row + 1, col, topRL) + middleRL.GetResponse(row - 1, col, topRL) - 2 * v;
            double dss = topRL.GetResponse(row, col) + bottomRL.GetResponse(row, col, topRL) - 2 * v;
            double dxy = (middleRL.GetResponse(row + 1, col + 1, topRL) - middleRL.GetResponse(row + 1, col - 1, topRL) -
                          middleRL.GetResponse(row - 1, col + 1, topRL) + middleRL.GetResponse(row - 1, col - 1, topRL)) / 4f;
            double dxs = (topRL.GetResponse(row, col + 1) - topRL.GetResponse(row, col - 1) -
                          bottomRL.GetResponse(row, col + 1, topRL) + bottomRL.GetResponse(row, col - 1, topRL)) / 4f;
            double dys = (topRL.GetResponse(row + 1, col) - topRL.GetResponse(row - 1, col) -
                          bottomRL.GetResponse(row + 1, col, topRL) + bottomRL.GetResponse(row - 1, col, topRL)) / 4f;

            var h = new double[3, 3];
            h[0, 0] = dxx;
            h[0, 1] = dxy;
            h[0, 2] = dxs;
            h[1, 0] = dxy;
            h[1, 1] = dyy;
            h[1, 2] = dys;
            h[2, 0] = dxs;
            h[2, 1] = dys;
            h[2, 2] = dss;
            return h;


        }

        private double[,] BuildDerivative(int row, int col, ResponseLayer topRL, ResponseLayer middleRL, ResponseLayer bottomRL)
        {
            double dx = (middleRL.GetResponse(row, col + 1, topRL) - middleRL.GetResponse(row, col - 1, topRL)) / 2f;
            double dy = (middleRL.GetResponse(row + 1, col, topRL) - middleRL.GetResponse(row - 1, col, topRL)) / 2f;
            double ds = (topRL.GetResponse(row, col) - bottomRL.GetResponse(row, col, topRL)) / 2f;

            double[,] d = { { dx }, { dy }, { ds } };
            return d;
        }

        /// <summary>
        /// Is the point at row,col in the middle layer is extremum(http://en.wikipedia.org/wiki/Maxima_and_minima) in 3x3x3 neighbourhood
        /// </summary>
        /// <param name="row"></param>
        /// <param name="col"></param>
        /// <param name="topRL"></param>
        /// <param name="middleRL"></param>
        /// <param name="bottomRL"></param>
        /// <returns></returns>
        private bool IsExtremum(int row, int col, ResponseLayer topRL, ResponseLayer middleRL, ResponseLayer bottomRL)
        {
            // make sure its within the bounds
            int layerBorder = (topRL.Filter + 1) / (2 * topRL.Step);
            if (row <= layerBorder || row >= topRL.Height - layerBorder || col <= layerBorder || col >= topRL.Width - layerBorder)
                return false;

            float candidate = middleRL.GetResponse(row, col, topRL);
            if (candidate < _threshold)
                return false;

            for (int rr = -1; rr <= 1; ++rr)
                for (int cc = -1; cc <= 1; ++cc)
                    // if we get a response of 3x3x3 is greater candiate then maxium
                    if (topRL.GetResponse(row + rr, col + cc) >= candidate ||
                        ((rr != 0 || cc != 0) && middleRL.GetResponse(row + rr, col + cc, topRL) >= candidate) ||
                        bottomRL.GetResponse(row + rr, col + cc, topRL) >= candidate)
                        return false;

            return true;
        }


        private List<ResponseLayer> BuildResponseMaps(int width, int height)
        {
            var responseMaps = new List<ResponseLayer>();

            int w = width / _sampleSize;
            int h = height / _sampleSize;
            int s = _sampleSize;


            // calc the approximate determiant of the hessian values
            if (_octaves >= 1)
            {
                responseMaps.Add(new ResponseLayer(w, h, s, 9));
                responseMaps.Add(new ResponseLayer(w, h, s, 15));
                responseMaps.Add(new ResponseLayer(w, h, s, 21));
                responseMaps.Add(new ResponseLayer(w, h, s, 27));
            }

            if (_octaves >= 2)
            {
                responseMaps.Add(new ResponseLayer(w / 2, h / 2, s * 2, 39));
                responseMaps.Add(new ResponseLayer(w / 2, h / 2, s * 2, 51));
            }

            if (_octaves >= 3)
            {
                responseMaps.Add(new ResponseLayer(w / 4, h / 4, s * 4, 75));
                responseMaps.Add(new ResponseLayer(w / 4, h / 4, s * 4, 99));
            }

            if (_octaves >= 4)
            {
                responseMaps.Add(new ResponseLayer(w / 8, h / 8, s * 8, 147));
                responseMaps.Add(new ResponseLayer(w / 8, h / 8, s * 8, 195));
            }

            if (_octaves >= 5)
            {
                responseMaps.Add(new ResponseLayer(w / 16, h / 16, s * 16, 291));
                responseMaps.Add(new ResponseLayer(w / 16, h / 16, s * 16, 387));
            }

            var rCount = responseMaps.Count;

            for (var i = 0; i < rCount; i++)
            {
                BuildResponseLayer(responseMaps[i]);
            }

            return responseMaps;
        }

        private void BuildResponseLayer(ResponseLayer responseMap)
        {
            int step = responseMap.Step;                // step size for this filter
            int b = (responseMap.Filter - 1) / 2 + 1;   // border for this filter
            int l = responseMap.Filter / 3;             // lobe for this filter
            int w = responseMap.Filter;                 // filter size
            float inverseArea = 1f / (w * w);           // normalisation factor

            for (int ar = 0, index = 0; ar < responseMap.Height; ++ar)
            {
                for (int ac = 0; ac < responseMap.Width; ++ac, index++)
                {
                    // calc image coord
                    int r = ar * step;
                    int c = ac * step;

                    // calc response components
                    float dxx = _integralImage.BoxIntegral(r - l + 1, c - b, 2 * l - 1, w) -
                                _integralImage.BoxIntegral(r - l + 1, c - l / 2, 2 * l - 1, l) * 3;

                    float dyy = _integralImage.BoxIntegral(r - b, c - l + 1, w, 2 * l - 1) -
                                _integralImage.BoxIntegral(r - l / 2, c - l + 1, l, 2 * l - 1) * 3;

                    float dxy = +_integralImage.BoxIntegral(r - l, c + 1, l, l) +
                                _integralImage.BoxIntegral(r + 1, c - l, l, l) -
                                _integralImage.BoxIntegral(r - l, c - l, l, l) -
                                _integralImage.BoxIntegral(r + 1, c + 1, l, l);

                    // normalise the filter respones with respect to their size
                    dxx *= inverseArea;
                    dyy *= inverseArea;
                    dxy *= inverseArea;

                    // get the determinat of hessian ersponse & laplacian sign
                    responseMap.Responses[index] = (dxx * dyy - 0.81f * dxy * dxy);
                    responseMap.Laplacian[index] = (byte)(dxx + dyy >= 0 ? 1 : 0);
                }
            }
        }

        private class ResponseLayer
        {
            public int Width { get; private set; }
            public int Height { get; private set; }
            public int Step { get; private set; }
            public int Filter { get; private set; }
            public float[] Responses { get; private set; }
            public byte[] Laplacian { get; private set; }

            public ResponseLayer(int width, int height, int step, int filter)
            {
                Width = width;
                Height = height;
                Step = step;
                Filter = filter;

                Responses = new float[width * height];
                Laplacian = new byte[width * height];
            }

            public float GetResponse(int row, int col, ResponseLayer responseLayer)
            {
                var scale = Width / responseLayer.Width;
                return Responses[(scale * row) * Width + (scale * col)];
            }

            public float GetResponse(int row, int col)
            {
                return Responses[row * Width + col];
            }

            public byte GetLaplacian(int row, int col, ResponseLayer responseLayer)
            {
                int scale = Width / responseLayer.Width;
                return Laplacian[(scale * row) * Width + (scale * col)];
            }
        }
    }
}
